#include "VoxelCollisionDetector.h"
#include "DecoSolidObject.h"
#include "DecoArticulatedBody.h"
#include "Scene.h"
#include "timer.h"
#include "DecoLogger.h"
#include "OpenGLRenderInterface.h"
#include "RenderMiscs.h"



#define VOXEL_RESOLUTION 50

CollidingVoxel::CollidingVoxel(const Index3& id, const vector3& pos, double gridSize) : 
mId(id),
mPos(pos),
mGridSize(gridSize),
mCollisionType(CT_Self),
mObj1(NULL),
mObj2(NULL),
mBody1(-1),
mBody2(-1)
{

}

VoxelCollisionDetector::VoxelCollisionDetector() : mScene(NULL), mVoxelGrid(NULL)
{

}
VoxelCollisionDetector::~VoxelCollisionDetector()
{
	if (mVoxelGrid)
	{
		Free3DArray(mVoxelGrid, mXRes, mYRes, mZRes);
	}
}
void VoxelCollisionDetector::SetScene(DecoScene* scene)
{
	if (scene == mScene)
		return;
	mScene = scene;
}

double VoxelCollisionDetector::collisionDetectionForBodies(int* bodyArray1, int* bodyArray2, int numArray1, int numArray2)
{
	DecoArticulatedObject* articulatedObj = mScene->GetArticulatedObject();
	MemSet3DArray<BYTE>(mVoxelGrid, 255, mXRes, mYRes, mZRes);
	const Box bb = articulatedObj->GetRenderData()->GetBoundingBox();
	vector3 bMin = bb.GetMin();
	vector3 bMax = bb.GetMax();

	int numOverlappingCells = 0;
	DecoSkelMesh* skelMesh = static_cast<DecoSkelMesh*>(articulatedObj->GetRenderData());

	for (int i = 0; i < mXRes; ++i)
	{
		for (int j = 0; j < mYRes; ++j)
		{
			for (int k = 0; k < mZRes; ++k)
			{
				vector3 samplePoint = bMin + vector3((i + 0.5) * mXStep, (j + 0.5) * mYStep, (k + 0.5) * mZStep);
				for (int ithBodyInArray1 = 0; ithBodyInArray1 < numArray1; ++ithBodyInArray1)
				{
					if (skelMesh->InsideIthBody(bodyArray1[ithBodyInArray1], samplePoint, true))
					{
						mVoxelGrid[i][j][k] = bodyArray1[ithBodyInArray1];
					}
				}
			}
		}
	}
	for (int i = 0; i < mXRes; ++i)
	{
		for (int j = 0; j < mYRes; ++j)
		{
			for (int k = 0; k < mZRes; ++k)
			{
				vector3 samplePoint = bMin + vector3((i + 0.5) * mXStep, (j + 0.5) * mYStep, (k + 0.5) * mZStep);
				for (int ithBodyInArray2 = 0; ithBodyInArray2 < numArray2; ++ithBodyInArray2)
				{
					if (mVoxelGrid[i][j][k] != 255 && skelMesh->InsideIthBody(bodyArray2[ithBodyInArray2], samplePoint, true))
					{
						numOverlappingCells++;
						mCollidingVoxelsForVis.push_back(Index3(i, j, k));
					}
				}
			}
		}
	}

	return numOverlappingCells * mXStep * mYStep * mZStep;

}
double VoxelCollisionDetector::CollisionDetection()
{
	SceneBuilderInfo info = DecoConfig::GetSingleton()->GetSceneBuilderInfo();
	if (info.mModelFileName != "../models/swim/frog.skel")
		return 0.0;
	DecoTimer timer;
	timer.startTimer();
	if (!mScene)	
		return 0.0;
	int checkLeg1[3] = {7 ,8, 9}, checkLeg2[3]= {10, 11, 12}, checkArm1[3] = {1, 2, 3}, checkArm2[3] = {4, 5, 6};
	int checkFoot = 12, checkBody = 0, checkHand = 6;
	DecoArticulatedObject* articulatedObj = mScene->GetArticulatedObject();
	const Box bb = articulatedObj->GetRenderData()->GetBoundingBox();
	vector3 bMin = bb.GetMin();
	vector3 bMax = bb.GetMax();
	int xRes = VOXEL_RESOLUTION;
	double xStep = (bMax.x - bMin.x) / xRes;
	int yRes = static_cast<int>((bMax.y - bMin.y) / xStep);
	double yStep = (bMax.y - bMin.y) / yRes;
	int zRes = static_cast<int>((bMax.z - bMin.z) / xStep);
	double zStep = (bMax.z - bMin.z) / zRes;
	if (mVoxelGrid)
	{
		Free3DArray(mVoxelGrid, mXRes, mYRes, mZRes);
	}
	mXRes = xRes;
	mYRes = yRes;
	mZRes = zRes;
	mXStep = xStep;
	mYStep = yStep;
	mZStep = zStep;
	mVoxelGrid = Malloc3DArray<BYTE>(mXRes, mYRes, mZRes);
	MemSet3DArray<BYTE>(mVoxelGrid, 255, mXRes, mYRes, mZRes);
	double numOverlappingCells = 0;
	mCollidingVoxelsForVis.clear();
	numOverlappingCells += collisionDetectionForBodies(checkLeg1, checkLeg2, 3, 3);
	numOverlappingCells += 2 * collisionDetectionForBodies(&checkFoot, &checkBody, 1, 1);
	numOverlappingCells += 2 * collisionDetectionForBodies(&checkHand, &checkBody, 1, 1);
	numOverlappingCells += collisionDetectionForBodies(checkArm1, checkArm2, 3, 3);
	//int numBodies = articulatedObj->GetNumBodies();
	//int numOverlappingCells = 0;
	//for (int i = 0; i < mXRes; ++i)
	//{
	//	for (int j = 0; j < mYRes; ++j)
	//	{
	//		for (int k = 0; k < mZRes; ++k)
	//		{
	//			vector3 samplePoint = bMin + vector3((i + 0.5) * mXStep, (j + 0.5) * mYStep, (k + 0.5) * mZStep);
	//			for (int ithBody = 0; ithBody < numBodies; ++ithBody)
	//			{
	//				DecoSkelMesh* skelMesh = static_cast<DecoSkelMesh*>(articulatedObj->GetRenderData());
	//				if (skelMesh->InsideIthBody(ithBody, samplePoint, true))
	//				{
	//					if (mVoxelGrid[i][j][k] == 255)
	//					{
	//						mVoxelGrid[i][j][k] = ithBody;
	//					}
	//					else
	//					{
	//						numOverlappingCells++;
	//						mCollidingVoxelsForVis.push_back(Index3(i, j, k));
	//					}
	//				}
	//			}
	//		}
	//	}
	//}
	double timeSpent = timer.getCurrentTime();
	(*DecoLogger::GetSingleton()) << "Collision Detection spent " << timeSpent << " seconds.\n";
	return numOverlappingCells;
}

void VoxelCollisionDetector::Render(DecoRenderInterface* RI, DecoLight** Lights, UINT numEffectiveLights)
{
	DecoArticulatedObject* articulatedObj = mScene->GetArticulatedObject();
	const Box& bb = articulatedObj->GetRenderData()->GetBoundingBox();

	DecoRenderMisc::GetSingleton()->DrawBox(NULL, bb, DecoColor(255, 255, 255));
	int numCollidingVoxels = static_cast<int>(mCollidingVoxelsForVis.size());
	for (int i = 0; i < numCollidingVoxels; ++i)
	{
		int xIndex = mCollidingVoxelsForVis[i].m_i;
		int yIndex = mCollidingVoxelsForVis[i].m_j;
		int zIndex = mCollidingVoxelsForVis[i].m_k;
		Box box(bb.GetMin() + vector3(xIndex * mXStep, yIndex * mYStep, zIndex * mZStep), bb.GetMin() + vector3((xIndex + 1) * mXStep, (yIndex + 1) * mYStep, (zIndex + 1) * mZStep));
		DecoColor color(0, 255, 0);	
		DecoRenderMisc::GetSingleton()->DrawBox(NULL, box, color);

	}

}
//double VoxelCollisionDetector::GetOverlapVolume()
//{
//	return 0.0;
//}
//void VoxelCollisionDetector::GetAllCollisionInfo(std::vector<CollidingVoxel>& collisionInfo)
//{
//
//}
//
